///////////////////////////////////////////////////////////////////////////////
//
//    Copyright 2010-2012 Geoffrey TOURON (geoffrey.touron@gmail.com)
//
//    This file is part of Chasis.
//
//    Chasis is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    Chasis is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with Chasis.  If not, see <http://www.gnu.org/licenses/>.
//
///////////////////////////////////////////////////////////////////////////////

#include "math/vec_util.hpp"
#include "actor.hpp"
#include "body.hpp"
#include "physicengine.hpp"

Physicengine::Physicengine() : _bodylist(0), faxis(0), saxis(1), taxis(2)
{

}

Physicengine::~Physicengine()
{

}

void		Physicengine::add(Body *bd)
{
	bd->prev = 0;
	if ((bd->next = _bodylist))
		_bodylist->prev = bd;
	_bodylist = bd;
}

void		Physicengine::remove(Body *bd)
{
	if (bd == _bodylist)
		_bodylist = _bodylist->next;
	if (bd->prev)
		bd->prev->next = bd->next;
	if (bd->next)
		bd->next->prev = bd->prev;
}

void		Physicengine::tick(float delta)
{
	Body	*bd;

	if (_bodylist)
	{
		for (bd = _bodylist; bd; bd = bd->next)
		{
			if (bd->dynamic)
			{
				bd->nextloc = bd->loc + bd->spd * delta; // + bd->ping
				bd->bot = min(bd->loc, bd->nextloc);
				bd->top = max(bd->loc + bd->size, bd->nextloc + bd->size);
			}
			else
			{
				bd->nextloc = bd->loc;
				bd->bot = bd->loc;
				bd->top = bd->loc + bd->size;
			}
		}
		_insertion_sort();
		bd = _bodylist;
		_check_collision(delta);
		for (; bd; bd = bd->next)
			bd->loc = bd->nextloc;
	}
}

void		Physicengine::_insertion_sort()
{
	Body	*i;
	Body	*j;
	Body	*bd;

	for (j = _bodylist->next; j;)
	{
		bd = j;
		j = j->next;
		for (i = bd->prev; i && bd->bot[faxis] < i->bot[faxis]; i = i->prev)
			;
		if (i != bd->prev)
		{
			if (bd->prev)
				bd->prev->next = bd->next;
			if (bd->next)
				bd->next->prev = bd->prev;
			if ((bd->prev = i))
			{
				bd->next = i->next;
				i->next->prev = bd;
				i->next = bd;
			}
			else
			{
				bd->next = _bodylist;
				_bodylist->prev = bd;
				_bodylist = bd;
			}
		}
	}
}

bool				Physicengine::_reaction(Body *x, Body *y, float t, unsigned int axis)
{
	vec<float, 2>	xloc;
	vec<float, 2>	yloc;

	xloc = x->loc + x->spd * t;
	yloc = y->loc + y->spd * t;
	for (unsigned int i = 0; i < 2; ++i)
	{
		if (i != axis && (xloc[i] > yloc[i] + y->size[i] || yloc[i] > xloc[i] + x->size[i]))
			return (false);
	}
	if (x->actor->collide(*y->actor) && x->dynamic)
		x->nextloc[axis] = xloc[axis];
	if (y->actor->collide(*x->actor) && y->dynamic)
		y->nextloc[axis] = yloc[axis];
	return (true);
}

void				Physicengine::_collide(float delta, Body *x, Body *y)
{
	vec<float, 2>	u;
	vec<float, 2>	v;

	u = (y->loc - (x->loc + x->size)) / (x->spd - y->spd);
	v = (x->loc - (y->loc + y->size)) / (y->spd - x->spd);
	for (unsigned int i = 0; i < 2; ++i)
	{
		float t = -1.0f;
		if (u[i] >= 0.0f && u[i] <= delta)
			t = v[i] >= 0.0f && v[i] < u[i] ? v[i] : u[i];
		else if (v[i] >= 0.0f && v[i] <= delta)
			t = v[i];
		if (t >= 0.0f && _reaction(x, y, t, i))
			break;
	}
}

void		Physicengine::_check_collision(float delta)
{
	Body	*i;
	Body	*j;

	for (j = _bodylist; j; j = j->next)
	{
		if (!j->actor->destroyed)
		{
			for (i = j->next; i; i = i->next)
			{
				if (!i->actor->destroyed)
				{
					if (j->top[faxis] > i->bot[faxis])
					{
						if ((i->dynamic || j->dynamic) && j->top[saxis] > i->bot[saxis] && i->top[saxis] > j->bot[saxis])
							// && j->top[taxis] > i->bot[taxis] && i->top[taxis] > j->bot[taxis])
							_collide(delta, j, i);
					}
					else
						break;
				}
			}
		}
	}
}
